﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using FarseerPhysics;
using Microsoft.Xna.Framework;
using FarseerPhysics.Dynamics;
using Polar.AI.Common;
using Polar.AI.Analysis;

namespace Polar.AI.Steering
{
    /// <summary>
    /// TODO : reorganize this code away from static class.  This is just here to test the individual methods.
    /// </summary>
    public static class SteeringLibrary
    {
        

        #region Breaking Steering...
        /// <summary>
        /// Returns an appropriate breaking force if the turn is greater then a given angle.
        /// </summary>
        /// <remarks>This simulates breaking as you enter a turn.  Note that we return a force not a unit vector.</remarks>
        public static Vector2 SteerForTurnBraking(IBody source, Vector2 newDirection, float withinRadian)
        {
            if (newDirection.IsZero())
                return Vector2.Zero;

            float directionAngle = newDirection.ToRadians();
            float angleDiff = Math.Abs(directionAngle - source.Direction());
            if (angleDiff > withinRadian)
            {
                return Vector2.Negate(source.LinearVelocity);
            }
            else
            {
                return Vector2.Zero;
            }
        }

        /// <summary>
        /// Returns exact opposite force from the current forward motion.
        /// </summary>
        public static Vector2 SteerForBraking(IBody source)
        {
            return Vector2.Negate(source.LinearVelocity);
        }
        #endregion

        #region Direction Facing...
        /// <summary>
        /// Will rotate a body towards it's linear velocity as turn speed of 0.025f radians per frame.
        /// </summary>
        public static float SteerToFaceFront(IBody source)
        {
            return SteerToFace(source, source.LinearVelocity.ToRadians(), 0.025f);
        }

        /// <summary>
        /// Will rotate a body toward a target at a given turn speed
        /// </summary>
        /// <param name="turnSpeed">In terms of radians per frame. Ex: 0.025f</param>
        public static float SteerToFace(IBody source, Vector2 target, float turnSpeed)
        {
            float x = target.X - source.Position.X;
            float y = target.Y - source.Position.Y;

            float desiredAngle = (float)Math.Atan2(y, x);

            return SteerToFace(source, desiredAngle, turnSpeed);
        }

        /// <summary>
        /// Will rotate a body toward a target at a given turn speed
        /// </summary>
        /// <param name="turnSpeed">In terms of radians per frame. Ex: 0.025f</param>
        public static float SteerToFace(IBody source, float direction, float turnSpeed)
        {
            float currentAngle = source.Direction();

            float difference = Calc.WrapAngle(direction - currentAngle);
            difference = MathHelper.Clamp(difference, -turnSpeed, turnSpeed);

            return Calc.WrapAngle(currentAngle + difference);
        }
        #endregion

        #region Steering Vectors...
        /// <summary>
        /// Returns unit vector based on the averaged position of neighbors that are within a distance and angle.
        /// </summary>
        /// <param name="cosMaxAngle">Cohesion angle.  Suggested value: -0.15f</param>
        public static Vector2 SteerForCohesion(IBody source, IBody[] flock, float maxDistance, float cosMaxAngle)
        {
            // steering accumulator and count of neighbors, both initially zero
            Vector2 steering = Vector2.Zero;
            int neighbors = 0;

            // for each of the other vehicles...
            for (int i = 0; i < flock.Length; i++)
            {
                IBody target = flock[i];
                if (SteeringQueries.IsInBodyNeighborhood(source, target, source.Radius + target.Radius, maxDistance, cosMaxAngle))
                {
                    // accumulate sum of neighbor's positions
                    steering += target.Position;

                    // count neighbors
                    neighbors++;
                }
            }

            // divide by neighbors, subtract off current position to get error-
            // correcting direction, then normalize to pure direction
            if (neighbors > 0)
            {
                steering = ((steering / (float)neighbors) - source.Position);
                steering.Normalize();
            }

            return steering;
        }

        /// <summary>
        /// Returns unit vector which indicates direction to move to keep a certain radial distance from neighbors.
        /// </summary>
        public static Vector2 SteerForSeparation(IBody source, IBody[] flock, float radius)
        {
            //if we are not moving, no steering required
            if (source.LinearVelocity == new Vector2(0f, 0f))
                return Vector2.Zero;

            // steering accumulator and count of neighbors, both initially zero
            Vector2 steering = Vector2.Zero;
            int neighbors = 0;

            // for each of the other vehicles...
            for (int i = 0; i < flock.Length; i++)
            {
                IBody other = flock[i];
                if (source == other)
                    continue;

                if (SteeringQueries.IsPositionInCircle(other.Position, source.Position, radius))
                {
                    // add in steering contribution
                    // (opposite of the offset direction, divided once by distance
                    // to normalize, divided another time to get 1/d falloff)
                    Vector2 offset = other.Position - source.Position;
                    steering += (offset / -offset.LengthSquared());

                    neighbors++;
                }
            }

            // divide by neighbors, then normalize to pure direction
            if (neighbors > 0)
            {
                steering = (steering / (float)neighbors);
                steering.Normalize();
            }

            return steering;
        }

        /// <summary>
        /// Returns a unit vector that points directly at target.
        /// </summary>
        public static Vector2 SteerForSeek(IBody source, Vector2 target)
        {
            return Vector2.Normalize(target - source.Position);
        }

        /// <summary>
        /// Returns a unit vector which performs an emergency collision avoidance
        /// </summary>
        public static SteeringAdvice SteerToAvoidCollision(IBody source, ThreatAnalysis analysis)
        {
            //TODO :  does this turn the body in the right direction?  
            //might need to do perpendicular turn from collision angle?
            return new SteeringAdvice()
            {
                Analysis = analysis,
                Threat = analysis.Target,
                ApplySteering = true,
                SteeringDirection = -analysis.SourceAngleToTarget.ToVector2()
            };
        }

        /// <summary>
        /// Performs a hard steer away from the nearest neighbor that comes within minSeparationDistance.
        /// </summary>
        /// <remarks>Make sure you only send neignbors that are in front of the body.  Unless you wish to steer from those behind you.</remarks>
        public static SteeringAdvice SteerToAvoidCloseNeighbors(IBody source, IBody[] flock, float minSeparationDistance)
        {
            SteeringAdvice advice = new SteeringAdvice();
            float minDistanceSquarded = minSeparationDistance * minSeparationDistance;

            advice.Threat = SteeringQueries.FindNearestNeighbor(source, flock);
            if (advice.Threat != null)
            {
                Vector2 offset = advice.Threat.Position - source.Position;
                if (offset.LengthSquared() < minDistanceSquarded)
                {
                    advice.SteeringDirection = Calc.PerpendicularComponent(-offset, source.Forward());
                    advice.ApplySteering = true;
                }
            }
           
            //no avoidance needed
            return advice;
        }

        /// <summary>
        /// Returns unit vector that steers to avoid neighbors in our general visinity.
        /// </summary>
        /// <param name="minAvoidDistance">The distance to look ahead to start avoiding a collision.</param>
        /// <param name="minEmergencyDistance">The distance to start to panic and apply emergency avoidance.</param>
        public static SteeringAdvice SteerToAvoidNeighbors(IBody source, IBody[] flock, float minAvoidDistance, float minEmergencyDistance)
        {
            SteeringAdvice advice = new SteeringAdvice();

            ThreatAnalysis analysis = SteeringQueries.PredictNearestBodyCollision(source, flock, minAvoidDistance);

            if (analysis != null) //potential collision found
            {
                advice.Analysis = analysis;
                advice.Threat = analysis.Target;

                //check for emergency avoid
                if (analysis.CollisionVector.IntersectDistance < minEmergencyDistance)
                    advice = SteerToAvoidCollision(source, analysis);
                else
                {
                    //TODO : each one of these "reactions" need to be tested
                    switch (analysis.CollisionOrientation)
                    {
                        case EnumCollisionOrientation.HeadOn:
                            advice.SteeringDirection = (-(analysis.SourceAngleToTarget + 1.57f)).ToVector2(); //90 from collission angle
                            advice.ApplySteering = true;
                            break;
                        case EnumCollisionOrientation.PerpendicularMajor: //we are faster
                            //advice.SteeringDirection = Vector2.Normalize(Calc.Perpendicular(body.Position, analysis.SeparationVector));
                            //we are faster so angle out of the way, TOTO : need to add side +/- factor (left or right)
                            advice.SteeringDirection = source.Forward() + Calc.Radian45.ToVector2();
                            advice.ApplySteering = true;
                            break;
                        case EnumCollisionOrientation.PerpendicularMinor: //we are slower
                            advice.SteeringDirection = -source.Forward();  //slow down a little
                            advice.ApplySteering = true;
                            break;
                        case EnumCollisionOrientation.Overtaking:
                            advice.SteeringDirection = (-analysis.SourceAngleToTarget).ToVector2(); //go opposit direction of intersect angle
                            //apply braking to match target
                            //advice.SteeringDirection = SteerToMatchSpeed(body, analysis.Target);
                            //advice.SteeringDirection = -body.Forward();
                            advice.ApplySteering = true;
                            break;
                    }
                }
            }

            return advice;
        }
        #endregion
    }
}
